`timescale 1ns / 1ps
/*--------------------------------------------------------------------*\
FileName        : psm_pid.v
Author          ：hpy
Email           ：yuan_hp@qq.com
Date            ：2024年07月06日
Description     ：
\*--------------------------------------------------------------------*/
module psm_pid (
    input clk,
    input rst_n , 
    input [7:0] kp ,
    input [7:0] ki ,
    input [7:0] kd ,
    input signed [15:0] target ,

    input signed [15:0] i_data , // 采集的实际值

    output reg signed [15:0] o_value, // PID计算的输出控制量
    output reg o_rdy // 输出有效
);
 
/* ------------------ function -------------------- */
 
/* -------------------- param --------------------- */
 
/*---------------------- reg ---------------------- */
reg  signed [15:0] pid_outval = 0  ;
reg  signed [15:0] speed = 0  ;
reg  signed [7:0]  pid_outval_L = 0;
/*----------------------- wire ---------------------*/
 
/*--------------------- assign ---------------------*/

/*---------------------- blk -----------------------*/
 
 
wire [7:0] out_port , port_id ; 
reg  [7:0] in_port ;
wire write_strobe  ;

psm_soc #(
    .memfile("psm_pid.hex") ,  
    .memnum ( 1024)  , 
	.stack_size  (32) ,  // 1 ~ 32 
	.scratch_size( 256) // 1 ~ 256  
) psm_soc_u1 (
    .clk (clk) ,
    .reset(~rst_n ) ,
    .in_port (in_port ),
    .out_port(out_port) ,
    .port_id (port_id ),
    .write_strobe(write_strobe) ,
    .interrupt() ,
    .interrupt_ack()
); 



always@ (posedge clk) begin
    o_rdy <= 1'b0;
    if(write_strobe)begin //
        case(port_id)
            // PID 
            // 输出值 L
            8'h04:  begin 
                pid_outval_L  <= out_port;
            end 
            8'h05:  begin 
                o_value <= {out_port , pid_outval_L };
                o_rdy <= 1'b1 ;
            end 
            default:  ;
        endcase 
    end 
end  


//输入口
always @(*)begin
    case(port_id)
        8'h02: in_port = i_data[7:0];
        8'h03: in_port = i_data[15:8];
        8'h06: in_port = kp;
        8'h07: in_port = ki;
        8'h08: in_port = kd;
        8'h09: in_port = target[7:0];
        8'h0a: in_port = target[15:8];
        default:in_port = 8'h00;
    endcase
end

endmodule
 
